#pragma config(Hubs,  S1, HTMotor,  none,     none,     none)
#pragma config(Hubs,  S2, HTMotor,  none,     none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Motor,  motorA,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorB,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  motorC,           ,             tmotorNXT, openLoop)
#pragma config(Motor,  mtr_S1_C1_1,     motor2,        tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     motor4,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C1_1,     motor3,        tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S2_C1_2,     motor1,        tmotorTetrix, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                           Tele-Operation Mode Code Template
//
// This file contains a template for simplified creation of an tele-op program for an FTC
// competition.
//
// You need to customize two functions with code unique to your specific robot.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                    initializeRobot
//
// Prior to the start of tele-op mode, you may want to perform some initialization on your robot
// and the variables within your program.
//
// In most cases, you may not have to add any code to this function and it will remain "empty".
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

void initializeRobot()
{
	// Place code here to sinitialize servos to starting positions.
	// Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.

	return;
}


/////////////////////////////////////////////////////////////////////////////////////////////////////
//
//                                         Main Task
//
// The following is the main code for the tele-op robot operation. Customize as appropriate for
// your specific robot.
//
// Game controller / joystick information is sent periodically (about every 50 milliseconds) from
// the FMS (Field Management System) to the robot. Most tele-op programs will follow the following
// logic:
//   1. Loop forever repeating the following actions:
//   2. Get the latest game controller / joystick settings that have been received from the PC.
//   3. Perform appropriate actions based on the joystick + buttons settings. This is usually a
//      simple action:
//      *  Joystick values are usually directly translated into power levels for a motor or
//         position of a servo.
//      *  Buttons are usually used to start/stop a motor or cause a servo to move to a specific
//         position.
//   4. Repeat the loop.
//
// Your program needs to continuously loop because you need to continuously respond to changes in
// the game controller settings.
//
// At the end of the tele-op period, the FMS will autonmatically abort (stop) execution of the program.
//
/////////////////////////////////////////////////////////////////////////////////////////////////////

task main()
{
	initializeRobot();

	waitForStart();   // wait for start of tele-op phase

	//Create "deadzone" variables. Adjust threshold value to increase/decrease deadzone
	int X2 = 0, Y1 = 0, X1 = 0, Y2 = 0, threshold = 10;
	int mid = 95, var = 45;


	while (true)
	{
		//Create "deadzone" for Y1/Ch3
		if(abs(joystick.joy1_y1) > threshold)
			Y1 = joystick.joy1_y1;
		else
			Y1 = 0;
		//Create "deadzone" for X1/Ch4
		if(abs(joystick.joy1_x1) > threshold)
			X1 = joystick.joy1_x1;
		else
			X1 = 0;
		//Create "deadzone" for X2/Ch1
		if(abs(joystick.joy1_x2) > threshold)
			X2 = joystick.joy1_x2;
		else
			X2 = 0;
		//Create "deadzone" for Y1/Ch3
		if(abs(joystick.joy1_y2) > threshold)
			Y2 = joystick.joy1_y2;
		else
			Y2 = 0;


		if(	( (X1<mid+var && X1>mid-var) && (Y1<mid+var && Y1>mid-var) ) && ( (X2<mid+var && X2>mid-var) && (Y2<mid+var && Y2>mid-var) ) )
		{
			float average = (X1+Y1+X2+Y2)/4;
			motor[motor1] = 0;
			motor[motor2] = average;
			motor[motor3] = 0;
			motor[motor4] = average;
		}

		else if( ( (X1<-mid+var && X1>-mid-var) && (Y1<-mid+var && Y1>-mid-var) ) && ( (X2<-mid+var && X2>-mid-var) && (Y2<-mid+var && Y2>-mid-var) ) )
		{
			float average = (X1+Y1+X2+Y2)/4;
			motor[motor1] = 0;
			motor[motor2] = average;
			motor[motor3] = 0;
			motor[motor4] = average;
		}

		else if( ( (X1<mid+var && X1>mid-var) && (Y1<-mid+var && Y1>-mid-var) ) && ( (X2<mid+var && X2>mid-var) && (Y2<-mid+var && Y2>-mid-var) ) )
		{
			float average = (-X1+Y1-X2+Y2)/4;
			motor[motor1] = average;
			motor[motor2] = 0;
			motor[motor3] = average;
			motor[motor4] = 0;
		}

		else if( ( (X1<-mid+var && X1>-mid-var) && (Y1<mid+var && Y1>mid-var) ) && ( (X2<-mid+var && X2>-mid-var) && (Y2<mid+var && Y2>mid-var) ) )
		{
			float average = (-X1+Y1-X2+Y2)/4;
			motor[motor1] = average;
			motor[motor2] = 0;
			motor[motor3] = average;
			motor[motor4] = 0;
		}
		else
		{
			motor[motor1] = Y1+X1;
			motor[motor4] = Y1-X1;

			motor[motor2] = Y2-X2;
			motor[motor3] = Y2+X2;
		}
	}
}
